#include <LiquidCrystal.h>
#define Kp 1.65
#define Ki 0.08
#define Kd 0.1

LiquidCrystal lcd = LiquidCrystal(16,17,18,20,21,22,23);
char * str1 = "SP=";
char * str2 = "MV=";
char * str3 = "e=";
char * str4 = "p=";

float P;
float I;
float D;

int pe;
int mv;
int sp;
int e;

volatile int power;

void setup(){
  //analogReference();
  lcd.begin(16,2);
  lcd.clear();
  pe = 0;P = 0;I = 0;D = 0;
  sp = 0;mv = 0;
}
void loop(){
  lcd.clear();
  sp = analogRead(0) * 100;
  //mv=analogRead(1);
  mv = (10 * analogRead(1))+mv + (0.5 * power); //mencoba feedback (tes osilasi)
  e = sp - mv;
  if(abs(e) < 2){
    e=0;
  }
  P = Kp * e;
  I= I + e;
  I= Ki * I;
  D=Kd * (e - pe);
  pe = e;
  power = (int)(P + I + D);
  lcd.print(str1);lcd.print(sp);
  lcd.setCursor(8,0);
  lcd.print(str3);lcd.print(e);
  lcd.setCursor(0,1);
  lcd.print(str2);lcd.print(mv);
  lcd.setCursor(8,1);
  lcd.print(str4);lcd.print(power);
  delay(800);
}
